/**
 * @file
 * @author dousha
 * @date 2020-12-31
 */
#include "mwc_protocol.h"

#include <string.h>
#if MWP_DYNAMIC_ALLOCATION == 1
#include <stdlib.h>
#endif

static uint8_t state = STATE_INVALID;
static uint8_t writer = 0, reader = 0;
static MWPFrame frames[MWP_FRAME_BUFFER_LENGTH] = {0};
static FrameCallbackFunction callback;
static uint8_t totalLength = 0, writtenLength = 0, readLength = 0, checksum = 0;

void mwp_init(FrameCallbackFunction fn) {
	memset(frames, 0, MWP_FRAME_BUFFER_LENGTH * sizeof(MWPFrame));
	state = STATE_PREAMBLE_DOLLAR;
	callback = fn;
}

void mwp_finalize() {
	callback = NULL;
#if MWP_DYNAMIC_ALLOCATION == 1
	// TODO: clean up allocated memory
#endif
}

void mwp_advance(uint8_t x) {
	switch (state) {
		case STATE_PREAMBLE_DOLLAR:
			if (x == '$') {
				state = STATE_PREAMBLE_M;
			} else {
				state = STATE_PREAMBLE_DOLLAR;
			}
			break;
		case STATE_PREAMBLE_M:
			if (x == 'M') {
				state = STATE_PREAMBLE_DIRECTION;
			} else {
				state = STATE_PREAMBLE_DOLLAR;
			}
			break;
		case STATE_PREAMBLE_DIRECTION:
			frames[writer].direction = x;
			state = STATE_COMMAND;
			break;
		case STATE_COMMAND:
			frames[writer].command = x;
			state = STATE_DATA_LENGTH;
			break;
		case STATE_DATA_LENGTH:
			totalLength = x;
			if (totalLength == 0) {
				state = STATE_CHECKSUM;
			} else {
				writtenLength = 0;
				state = STATE_DATA;
			}
			break;
		case STATE_DATA:
			frames[writer].data[writtenLength] = x;
			if (++writtenLength == totalLength) {
				state = STATE_CHECKSUM;
			}
			break;
		case STATE_CHECKSUM:
			readLength = 0;
			for (; readLength < totalLength; readLength++) {
				checksum ^= frames[writer].data[readLength];
			}
			if (checksum == x) {
				if (++writer == MWP_FRAME_BUFFER_LENGTH) {
					writer = 0;
				}
				callback(frames + reader);
				if (++reader == MWP_FRAME_BUFFER_LENGTH) {
					reader = 0;
				}
			}
			state = STATE_PREAMBLE_DOLLAR;
			break;
		default:
			state = STATE_PREAMBLE_DOLLAR;
			break;
	}
}
